#!/usr/bin/python3
# -*- coding: utf-8 -*-
"""
远程控制服务器模块

该模块实现了一个TCP Socket服务器，用于接收和处理外部控制指令，
执行相应的机器人动作和控制功能。
"""

import socketserver
import threading
import time
import re
import logging
from typing import List, Any, Optional

from robot_core.motion_controller import motion_controller

# 配置日志
logging.basicConfig(
    level=logging.INFO, format="%(asctime)s - %(name)s - %(levelname)s - %(message)s"
)
logger = logging.getLogger(__name__)


class CommandError(Exception):
    """命令执行错误异常类"""

    def __init__(self, message: str, data: Optional[List[Any]] = None):
        self.message = message
        self.data = data
        super().__init__(self.message)


class ServoServer(socketserver.BaseRequestHandler):
    """舵机服务器处理类，处理客户端连接和指令"""

    def __init__(self, *args, **kwargs):
        super().__init__(*args, **kwargs)
        self.running = True

    def handle(self):
        """处理客户端连接和数据"""
        logger.info("客户端已连接")
        conn = self.request
        recv = b""
        recv_data = ""

        while self.running:
            try:
                recv = conn.recv(1024)  # 接收客户端数据
                recv_data = recv.decode()
                logger.debug(f"收到数据: {recv_data}")

                if not recv_data:
                    self.running = False
                    logger.info("客户端断开连接")
                    break

                # 分割接收到的数据
                rdata = recv_data.split("\r\n")

                # 判断收到的指令是否符合规则 (I开头加3位数字)
                rex = re.compile(r"^(I[0-9]{3}).*")
                data = rdata[0]
                match = rex.match(data)

                if match:
                    if not 0 == match.start() or not len(data) == match.end():
                        logger.warning("指令格式错误")
                        continue

                    # 解析指令
                    data = data.split("-")
                    cmd = data[0][1:5]  # 提取指令编号
                    params = data[1:]  # 提取参数

                    try:
                        cmd = int(cmd)
                        logger.debug(f"执行命令: {cmd}, 参数: {params}")

                        # 执行命令
                        self._execute_command(cmd, params, conn)

                    except CommandError as err:
                        logger.error(f"命令执行错误: {err.message}")
                        if err.data:
                            logger.error(f"错误数据: {err.data}")
                    except ValueError as e:
                        logger.error(f"参数错误: {e}")
                    except Exception as e:
                        logger.error(f"未知错误: {e}")

                else:
                    logger.warning(f"无效指令: {data}")

                if not self.running:
                    break

            except Exception as e:
                logger.error(f"处理数据时出错: {e}")
                break

    def _execute_command(self, cmd: int, params: List[str], conn: Any) -> None:
        """执行命令

        Args:
            cmd: 命令ID
            params: 命令参数
            conn: 客户端连接对象
        """
        try:
            # 转换参数为整数
            int_params = [int(p) for p in params]

            # 执行命令
            motion_controller.execute_command(cmd, int_params)

        except ValueError as e:
            raise CommandError(f"参数转换错误: {e}", params)
        except Exception as e:
            raise CommandError(f"命令执行错误: {e}", params)

    def finish(self):
        """客户端断开连接时调用"""
        self.running = False
        logger.info("客户端连接已关闭")


class RemoteServer(socketserver.ThreadingTCPServer):
    """远程控制TCP服务器类，支持多线程处理客户端连接"""

    allow_reuse_address = True  # 允许地址重用

    def __init__(self, *args, **kwargs):
        super().__init__(*args, **kwargs)
        self.running = True

    def shutdown(self):
        """关闭服务器"""
        self.running = False
        super().shutdown()


def init_robot():
    """初始化机器人姿态"""
    try:
        # 设置所有舵机到初始位置
        motion_controller._init_servos()
        time.sleep(0.5)
        logger.info("机器人初始化完成")
    except Exception as e:
        logger.error(f"机器人初始化失败: {e}")
        raise


def main():
    """主函数"""
    try:
        # 初始化机器人
        init_robot()

        # 启动TCP服务器，监听端口1444
        server = RemoteServer(("", 1444), ServoServer)

        # 创建服务器线程
        server_thread = threading.Thread(target=server.serve_forever)
        server_thread.daemon = True  # 设置为守护线程
        server_thread.start()

        logger.info("远程控制服务器已启动，监听端口1444")

        # 主线程保持运行
        while server.running:
            time.sleep(0.1)

    except KeyboardInterrupt:
        logger.info("收到退出信号，正在关闭服务器...")
        server.shutdown()
        server.server_close()
    except Exception as e:
        logger.error(f"服务器运行错误: {e}")
        if "server" in locals():
            server.shutdown()
            server.server_close()
    finally:
        logger.info("服务器已关闭")


if __name__ == "__main__":
    main()
